---@diagnostic disable: undefined-global
import("System")
import("System.IO.Ports")
import("Jodell.Tool", "Jodell.Tool")
--打开串口
COMID = "COM9"
slave_ID = 1
client=Lua_ModbusRtuClient()
client.PortName=COMID
client:Connect()

-- 创建 CSV 对象并打开

ret,SN = client:ReadString(slave_ID,562,16)
index=string.find(SN,"\0")
SN=string.sub(SN,1,index-1)
Info(string.format("设备编号%s",SN))
localpath = string.format("D:\\jodell_work\\Modbus_software\\钧舵调试助手 V1.0.6\\ForceCtrl\\推力测试(%s).csv",SN)
Info(localpath)
writer = Lua_CsvWriter(localpath)

writer:Open()

writer:WriteString("参考电流(A)")
writer:WriteSplitter()
writer:WriteString("反馈电流(A)")
writer:WriteSplitter()
writer:WriteString("测量压力(N)")
writer:WriteSplitter()
--writer:WriteString("设备反馈压力")
--writer:WriteSplitter()
writer:WriteString("设置电流(mA)")
writer:WriteSplitter()
writer:WriteLine()

--设备使能
ret = client:WriteUInt16(slave_ID,1002,1)	
if ret ~= 0 then
	Error("设置设备使能失败")
	goto exit
end
--设备复位
ret = client:WriteUInt16(slave_ID,1004,1)	
if ret ~= 0 then
	Error("设置设备复位失败")
	goto exit
end
--切换到标定模式
ret = client:WriteUInt16(slave_ID,0x013d,1)
if ret ~= 0 then
	Error("设置标定模式失败")
	goto exit
end
Delay(100)

ret = client:WriteUInt16(slave_ID,0x3f3,650)	
	if ret == 0 then
		Info(string.format("设置打开力:%dmA",650))
	else
		Error(string.format("设置打开力:%dmA失败",650))
		goto exit
	end
--开始测试
for torque = 100, 650, 10 do
	--终止测试
    	if (IsCancel) then
    		Warn("手动终止")
            goto exit
		break
	end
	--读取运行状态
	--ret,runstatus = client:ReadUInt16(1,2000)
	--if ret ~= 0 or runstatus ~= 4 then
		--Error("切换运行模式失败")
		--goto exit
	--end
	--设置推电流
	ret = client:WriteUInt16(slave_ID,1010,torque)	
	if ret == 0 then
		Info(string.format("设置夹持电流:%dmA",torque))
	else
		Error(string.format("设置夹持电流:%dmA失败",torque))
		goto exit
	end
	--设置速度
	
	--设置推动作
	ret = client:WriteUInt16(slave_ID,1003,0)	
	if ret ~= 0 then
		Error("设置夹持动作失败")
		goto exit
	end
	Delay(1500)
	--读电流参考
	ret,torque_ref = client:Read03FloatCDAB(slave_ID,114)
	if ret ~= 0 then
		Error("读电流参考失败")
		goto exit
	end
	Info(string.format("推电流参考:%fA",torque_ref))
	--读电流反馈
	ret,torque_fbk = client:Read03FloatCDAB(slave_ID,126)
	if ret ~= 0 then
		Error("读电流反馈失败")
		goto exit
	end
	Info(string.format("推电流反馈:%fA",torque_fbk))
	--读压力
	ret,torque_mes = client:Read04FloatABCD(99,0)
	if ret ~= 0 then
		ret,torque_mes = client:Read04FloatABCD(99,0)
		if ret ~= 0 then
			ret,torque_mes = client:Read04FloatABCD(99,0)
			if ret ~= 0 then
				Error("读压力测量失败")
				goto exit
			end
		end
	end
	torque_mes = torque_mes * 9.8
	Info(string.format("测量压力:%fN",torque_mes))
	--ret,force_fbk = client:ReadUInt16(1,2004)
	--if ret ~=0 then
	--	Error("读设备反馈压力失败")
	--	goto exit
	--end
	--Info(string.format("设备反馈压力:%dN",force_fbk))
	--保存数据
	writer:WriteFloat(-torque_ref)
	writer:WriteSplitter()
	writer:WriteFloat(-torque_fbk)
	writer:WriteSplitter()
	writer:WriteFloat(torque_mes)
	writer:WriteSplitter()
	--writer:WriteFloat(force_fbk)
	--writer:WriteSplitter()
	writer:WriteFloat(torque)
	writer:WriteSplitter()
	writer:WriteLine()
	Delay(100)
	--设置拉动作
	ret = client:WriteUInt16(slave_ID,1003,1)	
	if ret ~= 0 then
		Error("设置打开动作失败")
		goto exit
	end
	Delay(1000)
end

--关闭CSV文件
writer:Close()
client:Disconnect()

SalveNo = slave_ID --Modbus从站号
PortName = COMID --串口名称
fileName = localpath  --CSV文件路径
xIndex = 0 --电流 在文件中第几列，下标从0开始
yIndex = 2 --实际力 在文件中第几列，下标从0开始

pso_client = Lua_PsoClient() --线性拟合操作对象
client = Lua_ModbusRtuClient() --Modbus通讯对象
pso_client:LoadCsvData(fileName, xIndex, yIndex) --加载数据
pso_client:Optimize(1000) --迭代计算分割点
pso_client:FitData() --线性拟合
if IsCancel then
    return
end
--打印拟合信息
Info(string.format("共 %d 个分割点", pso_client.LineSegments.Count))

for i = 0, pso_client.LineSegments.Count - 1 do
    Info(string.format("K:%f \tB:%f \tMinValue:%f \tMaxValue:%f",
        pso_client.LineSegments[i].K,
        pso_client.LineSegments[i].B,
        pso_client.LineSegments[i].MinValue,
        pso_client.LineSegments[i].MaxValue))
    if IsCancel then
        return
    end
end
--Modbus从站连接
client.PortName = PortName
client:Connect()

if IsCancel then
    return
end

client:WriteUInt16(SalveNo, 446, pso_client.LineSegments.Count)                      --下发分段数
client:WriteFloatCDAB(SalveNo, 448, pso_client.LineSegments[0].MinValue)             --下发上边界
for i = 0, pso_client.LineSegments.Count - 1 do
    client:WriteFloatCDAB(SalveNo, 450 + i * 2, pso_client.LineSegments[i].MaxValue) --下发分割点
    client:WriteFloatCDAB(SalveNo, 460 + i * 2, pso_client.LineSegments[i].K)
    client:WriteFloatCDAB(SalveNo, 470 + i * 2, pso_client.LineSegments[i].B)

    if IsCancel then
        return
    end
end
client:WriteUInt16(SalveNo, 0x012C, 1) --保存参数
client:Disconnect()
Info("校准完成");
pso_client:SaveImage("./data.png")

--力控验证
Info("开始验证")
--
writer = Lua_CsvWriter(string.format("./ForceCtrl/验证结果(%s).csv",SN))
writer:Open()

writer:WriteString("参考电流(A)")
writer:WriteSplitter()
writer:WriteString("反馈电流(A)")
writer:WriteSplitter()
writer:WriteString("测量压力(N)")
writer:WriteSplitter()
--writer:WriteString("设备反馈压力")
--writer:WriteSplitter()
writer:WriteString("设置压力(N)")
writer:WriteSplitter()
writer:WriteLine()

client=Lua_ModbusRtuClient()
client.PortName=COMID
client:Connect()

--设备重启
ret = client:WriteUInt16(slave_ID,1000,1)
Delay(2000)

--设备使能
ret = client:WriteUInt16(slave_ID,1002,1)	
if ret ~= 0 then
	Error("设置设备使能失败")
	goto exit
end
--设备复位
ret = client:WriteUInt16(slave_ID,1004,1)	
if ret ~= 0 then
	Error("设置设备复位失败")
	goto exit
end
--切换到力控模式
ret = client:WriteUInt16(slave_ID,0x013d,0)
if ret ~= 0 then
	Error("设置力控模式失败")
	goto exit
end
Delay(100)

ret = client:WriteUInt16(slave_ID,1011,100)	
	if ret == 0 then
		Info(string.format("设置打开力:%dN",100))
	else
		Error(string.format("设置打开力:%dN失败",100))
		goto exit
	end
--开始测试
for torque = 50, 150, 1 do
	--终止测试
    	if (IsCancel) then
    		Warn("手动终止")
            goto exit
		break
	end
	--读取运行状态
	--ret,runstatus = client:ReadUInt16(1,2000)
	--if ret ~= 0 or runstatus ~= 4 then
		--Error("切换运行模式失败")
		--goto exit
	--end
	--设置推电流
	ret = client:WriteUInt16(slave_ID,1010,torque)	
	if ret == 0 then
		Info(string.format("设置夹持力:%dN",torque))
	else
		Error(string.format("设置夹持力:%dN失败",torque))
		goto exit
	end
	--设置夹持动作
	ret = client:WriteUInt16(slave_ID,1003,0)	
	if ret ~= 0 then
		Error("设置夹持动作失败")
		goto exit
	end
	Delay(1500)
	--读电流参考
	ret,torque_ref = client:Read03FloatCDAB(slave_ID,114)
	if ret ~= 0 then
		Error("读电流参考失败")
		goto exit
	end
	Info(string.format("推电流参考:%fA",torque_ref))
	--读电流反馈
	ret,torque_fbk = client:Read03FloatCDAB(slave_ID,126)
	if ret ~= 0 then
		Error("读电流反馈失败")
		goto exit
	end
	Info(string.format("推电流反馈:%fA",torque_fbk))
	--读压力
	ret,torque_mes = client:Read04FloatABCD(99,0)
	if ret ~= 0 then
		ret,torque_mes = client:Read04FloatABCD(99,0)
		if ret ~= 0 then
			ret,torque_mes = client:Read04FloatABCD(99,0)
			if ret ~= 0 then
				Error("读压力测量失败")
				goto exit
			end
		end
	end
	torque_mes = torque_mes * 9.8
	Info(string.format("测量压力:%fN",torque_mes))

    writer:WriteFloat(-torque_ref)
	writer:WriteSplitter()
	writer:WriteFloat(-torque_fbk)
	writer:WriteSplitter()
	writer:WriteFloat(torque_mes)
	writer:WriteSplitter()
	--writer:WriteFloat(force_fbk)
	--writer:WriteSplitter()
	writer:WriteFloat(torque)
	writer:WriteSplitter()
	writer:WriteLine()
	Delay(100)

    ret = client:WriteUInt16(slave_ID,1003,1)	
	if ret ~= 0 then
		Error("设置打开动作失败")
		goto exit
	end
	Delay(1000)
end

--退出
::exit:: 
--关闭CSV文件
writer:Close()
--关闭串口
client:Disconnect()
Info("测试结束")
Info("打开文件查看结果")